#!/usr/bin/env python3
"""
FishBot micro-ROS系统测试脚本
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import time

class MicroROSSystemTester(Node):
    def __init__(self):
        super().__init__('microros_system_tester')
        
        # 发布器
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        
        # 订阅器
        self.odom_sub = self.create_subscription(
            Odometry, 'odom', self.odom_callback, 10)
        
        # 状态
        self.odom_received = False
        self.test_start_time = time.time()
        
        self.get_logger().info("🧪 micro-ROS系统测试器启动")

    def odom_callback(self, msg):
        """里程计回调"""
        if not self.odom_received:
            self.odom_received = True
            self.get_logger().info("✅ 接收到FishBot里程计数据")
            self.get_logger().info(f"位置: x={msg.pose.pose.position.x:.3f}, y={msg.pose.pose.position.y:.3f}")

    def run_test_sequence(self):
        """运行测试序列"""
        self.get_logger().info("🚀 开始micro-ROS通信测试")
        
        # 测试命令序列
        test_commands = [
            ("前进", 0.2, 0.0, 2.0),
            ("停止", 0.0, 0.0, 1.0),
            ("左转", 0.0, 0.5, 2.0),
            ("停止", 0.0, 0.0, 1.0),
            ("右转", 0.0, -0.5, 2.0),
            ("停止", 0.0, 0.0, 1.0),
        ]
        
        for cmd_name, linear, angular, duration in test_commands:
            self.get_logger().info(f"🎮 执行: {cmd_name}")
            
            # 创建并发送命令
            twist = Twist()
            twist.linear.x = linear
            twist.angular.z = angular
            
            # 持续发送命令
            start_time = time.time()
            while time.time() - start_time < duration:
                self.cmd_vel_pub.publish(twist)
                rclpy.spin_once(self, timeout_sec=0.1)
            
            # 确保停止
            if linear != 0.0 or angular != 0.0:
                stop_twist = Twist()
                for _ in range(5):
                    self.cmd_vel_pub.publish(stop_twist)
                    time.sleep(0.1)
        
        # 测试结果
        total_time = time.time() - self.test_start_time
        self.get_logger().info("🎉 测试完成")
        self.get_logger().info(f"总测试时间: {total_time:.1f}秒")
        self.get_logger().info(f"里程计数据: {'✅ 正常' if self.odom_received else '❌ 未收到'}")

def main():
    print("🧪 FishBot micro-ROS系统测试")
    print("============================")
    print("此测试将:")
    print("1. 发送运动命令到 /cmd_vel 话题")
    print("2. 监听 /odom 话题的里程计数据")
    print("3. 验证micro-ROS通信是否正常")
    print("")
    
    input("按Enter开始测试...")
    
    rclpy.init()
    tester = MicroROSSystemTester()
    
    try:
        # 等待连接
        print("⏳ 等待5秒建立连接...")
        start_time = time.time()
        while time.time() - start_time < 5:
            rclpy.spin_once(tester, timeout_sec=0.5)
        
        # 运行测试
        tester.run_test_sequence()
        
    except KeyboardInterrupt:
        print("\n🛑 测试被用户中断")
    except Exception as e:
        print(f"\n❌ 测试出现错误: {e}")
    finally:
        tester.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()